
import lejos.nxt.LightSensor;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;

public class RobotModel {
	
	public static final int unitDistance = 33;
	
	private boolean lifted = false; //keeps the state of the robot lift
	
	private Motor lifterMotor = Motor.C;
	private Motor turnMotor = Motor.A;
	private Motor forwardMotor = Motor.B;
	
	private UltrasonicSensor leftSensor = new UltrasonicSensor(SensorPort.S1);
	private UltrasonicSensor rightSensor = new UltrasonicSensor(SensorPort.S3);
	private LightSensor frontSensor = new LightSensor(SensorPort.S4);
	
	private int direction = Direction.NORTH;
	
	public RobotModel() {
		
	}
	
	public int getFronSensorValue() {
		return frontSensor.getNormalizedLightValue();
	}
	
	public int getRightSensor() {
		return rightSensor.getDistance();
	}
	
	public int getLeftSensor() {
		return leftSensor.getDistance();
	}
	
	public void gotoEast() {
		gotoDirection(Direction.EAST);
	}
	
	public void gotoNorth() {
		gotoDirection(Direction.NORTH);
	}
	
	public void gotoWest() {
		gotoDirection(Direction.WEST);
	}
	
	public void gotoSouth() {
		gotoDirection(Direction.SOUTH);
	}
	
	public void gotoDirection(int wantedDirect) {
		if (wantedDirect == direction) {
			goForward();
		}
		else {
			int angle = Direction.getDirectionAngle(direction, wantedDirect);
			turn(angle);
			goForward();
			direction = wantedDirect;
		}
	}
	
	private void turn(int angle) {
		liftUp();
		//since gear ratio is 1:7
		turnMotor.rotate(angle *7, false);
		getDown();
	}
	
	private void liftUp() {
		lifterMotor.rotateTo(360, false);
		lifted = true;
	}
	
	private void getDown() {
		lifterMotor.rotateTo(0, false);
		lifted = false;
	}
	
	private void goForward() {
		int angle = (int) Math.round(360 * (unitDistance / (Math.PI * 5.5)));
		goAngle(angle);
	}
	
	private void goAngle(int angle) {
		forwardMotor.rotate((-1)*angle, false);
	}

}
